#ifdef __cplusplus
extern "C" {
#endif
#include "system/lowlevel_init.h"
#include "system/systemtime.h"

#ifdef __cplusplus

}
#endif

#include "bno055/BNO055.h"
#include "connectivity/SPI.h"
#include "mpu9250/mpu9250.h"
#include "system/debug.h"
#include "system/led.h"
#include <connectivity/UART.h>
#include "l3gd20/l3gd20.h"
#include "ms5611/ms5611.h"



uint32_t last_time = micros();

static BNO055 *pbno055;
static MPU9250 *pmpu9250;
static L3GD20 *pl3gd20;
static float q[4];


float acc[3], gyro[3], mag[3];

#define INTERGER_BOARD
int main() {

  LED led_green, led_blue;
#ifdef INTERGER_BOARD
  led_green.init(GPIOE, 2, std::string("green"));
  led_blue.init(GPIOE, 3, std::string("blue"));
  led_green.on();
  led_blue.off();;

#else
  led_blue.init(GPIOB, 9, std::string("blue"));
#endif

  UARTDriver::UART_INFO uart1_info{
      1,
      //      115200,
      110000,
      true,
      true,
      UARTDriver::UART_MOOD::T_RX,
      UARTDriver::GPIO::A,
      UARTDriver::GPIO::A,
      9, 10,
  };
  SPIDirver::SPI_INIT_INFO spi1_info{
      1,
      1,
      true,
      true,
      SPIDirver::GPIO::A,
      SPIDirver::GPIO::A,
      SPIDirver::GPIO::A,
      SPIDirver::GPIO::A,
      5, 6, 7, 4,
  };

  SPIDirver::SPI_INIT_INFO spi2_info{
      2,
      20,
      true,
      true,
      SPIDirver::GPIO::B,
      SPIDirver::GPIO::B,
      SPIDirver::GPIO::B,
      SPIDirver::GPIO::B,
      13, 14, 15, 12,
  };
  SPIDirver::SPI_INIT_INFO spi3_info{
      3,
      20,
      true,
      true,
      SPIDirver::GPIO::B,
      SPIDirver::GPIO::B,
      SPIDirver::GPIO::B,
      SPIDirver::GPIO::D,
      3, 4, 5, 7,
  };
  static UARTDriver uart1_driver(uart1_info);
  static SPIDirver spi1_driver(spi1_info);
  static SPIDirver spi2_driver(spi2_info);
  static SPIDirver spi3_driver(spi3_info);
  //
  static MPU9250 mpu9250([](const uint8_t *buffer, uint16_t len) {
                           spi1_driver.dma_read_bytes(buffer, len);
                           //                           spi1_driver.read_bytes(buffer, len);
                         },
                         [](const uint8_t *buffer, uint16_t len) {
                           spi1_driver.dma_write_bytes(buffer, len);
                           //                           spi1_driver.write_bytes(buffer, len);
                         },
                         [](const uint8_t *buffer, uint16_t len, void callback_fun(uint16_t)) {
                           spi1_driver.dma_async_read(buffer, len, callback_fun);
                         },
                         [](const uint8_t *buffer, uint16_t len, void callback_fun(uint16_t)) {
                           spi1_driver.dma_async_write(buffer, len, callback_fun);
                         },
                         DelayMs);

  static L3GD20 l3gd20([](const uint8_t *buffer, uint16_t len) {
                         spi3_driver.dma_read_bytes(buffer, len);
                         //                  spi3_driver.read_bytes(buffer, len);
                       },
                       [](const uint8_t *buffer, uint16_t len) {
                         spi3_driver.dma_write_bytes(buffer, len);
                         //                  spi3_driver.read_bytes(buffer, len);
                       },
                       [](const uint8_t *buffer, uint16_t len, void callback_fun(uint16_t)) {
                         spi3_driver.dma_async_read(buffer, len, callback_fun);
                       },
                       [](const uint8_t *buffer, uint16_t len, void callback_fun(uint16_t)) {
                         spi3_driver.dma_async_write(buffer, len, callback_fun);
                       },
                       DelayMs);
  //
  static BNO055 bno055([](const uint8_t *buffer, const uint8_t len) {
                         uart1_driver.dma_read_bytes(buffer, len);
                         //                         uart1_driver.read_bytes(buffer, len);
                       },
                       [](const uint8_t *buffer, const uint8_t len) {
                         uart1_driver.dma_write_bytes(buffer, len);
                         //                         uart1_driver.write_bytes(buffer, len);
                       },
                       [](const uint8_t *buffer, uint16_t len, void callback_fun(uint16_t)) {
                         uart1_driver.dma_async_read(buffer, len, callback_fun);
                       },
                       [](const uint8_t *buffer, uint16_t len, void callback_fun(uint16_t)) {
                         uart1_driver.dma_async_write(buffer, len, callback_fun);
                       },
                       DelayMs);
  static MS5611 ms5611(
      [](const uint8_t *buffer, uint16_t len) {
        spi2_driver.dma_read_bytes(buffer, len);
      },
      [](const uint8_t *buffer, uint16_t len) {
        spi2_driver.dma_write_bytes(buffer, len);
      },
      [](const uint8_t *buffer, uint16_t len, void callback_fun(uint16_t)) {
        spi2_driver.dma_async_read(buffer, len, callback_fun);
      },
      [](const uint8_t *buffer, uint16_t len, void callback_fun(uint16_t)) {
        spi2_driver.dma_async_write(buffer, len, callback_fun);
      },
      DelayMs);

  //  l3gd20.init(L3GD20::gyroRange_t::GYRO_RANGE_2000DPS);
  //  ms5611.init();
  //  mpu9250.init();
  bno055.begin(BNO055::bno055_opmode_t::OPERATION_MODE_NDOF);

  pbno055 = &bno055;
  pmpu9250 = &mpu9250;
  pl3gd20 = &l3gd20;


  //  bno055_read();
  //    mpu9250_read();

#pragma clang diagnostic push

#pragma ide diagnostic ignored "EndlessLoop"

  //  recieve(uart6_driver);
  while (true) {
    //    static uint8_t buffer[2];
    //    buffer[0] = MPU9250_WHO_AM_I | 0x80;
    //    static float acc[3], gyro[3], mag[3];
    //    static uint8_t data[22];
    //
    //    static float q[4];
    float time_period = (float) (micros() - last_time) / 1000000.0;
    last_time = micros();
    //        my_printf("time period: %f \n", time_period);
    //    //    mpu9250.get_9axis_data(acc,gyro,mag);

    DelayMs(500);
#ifdef INTERGER_BOARD
    led_green.toggle();
#endif
    led_blue.toggle();
    //    pbno055->async_get_quat(q, [](BNO055::uart_ack_type_t ec) {
    //      if (!ec) {
    //        my_printf("Q:%f %f %f %f \n", q[0], q[1], q[2], q[3]);
    //      } else {
    //        my_printf("ERRO:  %d\n", ec);
    //      }
    //    });
    //
    //        pmpu9250->async_get_9axis_data(acc, gyro, mag, []() {
    //          my_printf("ACC:%f %f %f   GYRO:%f %f %f   MAG:%f %f %f \n", acc[0], acc[1], acc[2], gyro[0], gyro[1], gyro[2], mag[0], mag[1], mag[2]);
    //        });
    //    pl3gd20->get_gyro_data(gyro);
    //    my_printf("GYRO:%f %f %f  \n", gyro[0], gyro[1], gyro[2]);
    //
    //    pmpu9250->get_9axis_data(acc, gyro, mag);
    //
    //    my_printf("ACC:%f %f %f   GYRO:%f %f %f   MAG:%f %f %f \n", acc[0], acc[1], acc[2], gyro[0], gyro[1], gyro[2], mag[0], mag[1], mag[2]);
    //    ////
    //    float pquat[4]{0};
    //    int8_t ret = pbno055->get_quat(pquat);
    //    if (ret == 0) {
    //      my_printf("QUAT:%f %f %f %f \n", pquat[0], pquat[1], pquat[2], pquat[3]);
    //
    //    } else {
    //      my_printf("ERRO: %d\n", ret);
    //    };
    //    float alt = ms5611.getEstimatedAltitude();
    //    my_printf("ALT:%f   \n", alt);
    //      my_printf("ACC:%f %f %f   GYRO:%f %f %f   MAG:%f %f %f Q:%f %f %f %f   %f\n", acc[0], acc[1], acc[2], gyro[0], gyro[1], gyro[2], mag[0], mag[1], mag[2], q[0], q[1], q[2], q[3], time_period);
    //      my_printf("hello\n");

    //    l3gd20.get_gyro_data(gyro);
    //        my_printf("hello\n");
  }
#pragma clang diagnostic pop

  return 0;
}












//
//
//
//spi_read =<std::_Maybe_unary_or_binary_function<void, unsigned char const*, unsigned short>> =
//    {<std::binary_function<unsigned char const*, unsigned short, void>> = {<No data fields>}, <No data fields>},
//
//<std::_Function_base> = {
//    static _M_max_size = 8,
//    static _M_max_align = 4,
//    _M_functor = {_M_unused = {_M_object = 0x20000638,_M_const_object = 0x20000638, _M_function_pointer = 0x20000638, _M_member_pointer = (void (std::_Undefined_class::*)(std::_Undefined_class * const)) 0x20000638},
//      _M_pod_data = "8\006\000 \000\000\000"},
//
//      _M_manager = 0x8002049 <std::_Function_base::_Base_manager<std::_Bind<void (SPI1Dirver::*(SPI1Dirver, std::_Placeholder<1>, std::_Placeholder<2>))(unsigned char const*, unsigned short)> >::_M_manager(std::_Any_data&, std::_Any_data const&, std::_Manager_operation)>},
//_M_invoker = 0x8002011 <std::_Function_handler<void (unsigned char const*, unsigned short), std::_Bind<void (SPI1Dirver::*(SPI1Dirver, std::_Placeholder<1>, std::_Placeholder<2>))(unsigned char const*, unsigned short)> >::_M_invoke(std::_Any_data const&, unsigned char const*&&, unsigned short&&)>,
//
//
//
//    spi_write = <std::_Maybe_unary_or_binary_function<void, unsigned char const*, unsigned short>> = {<std::binary_function<unsigned char const*, unsigned short, void>> = {<No data fields>}, <No data fields>}, <std::_Function_base> = {static _M_max_size = 8, static _M_max_align = 4, _M_functor = {_M_unused = {_M_object = 0x20000690,
//      _M_const_object = 0x20000690, _M_function_pointer = 0x20000690, _M_member_pointer = &virtual table offset 536872592, this adjustment -570691856},
//      _M_pod_data = "\220\006\000 \341\335", <incomplete sequence \367\273>},
//      _M_manager = 0x8002049 <std::_Function_base::_Base_manager<std::_Bind<void (SPI1Dirver::*(SPI1Dirver, std::_Placeholder<1>, std::_Placeholder<2>))(unsigned char const*, unsigned short)> >::_M_manager(std::_Any_data&, std::_Any_data const&, std::_Manager_operation)>},
//_M_invoker = 0x8002011 <std::_Function_handler<void (unsigned char const*, unsigned short), std::_Bind<void (SPI1Dirver::*(SPI1Dirver, std::_Placeholder<1>, std::_Placeholder<2>))(unsigned char const*, unsigned short)> >::_M_invoke(std::_Any_data const&, unsigned char const*&&, unsigned short&&)>,
//
//
//    spi_async_read = {<std::_Maybe_unary_or_binary_function<void, unsigned char const*, short unsigned int, std::function<void(short unsigned int)> >> = {<No data fields>}, <std::_Function_base> = {static _M_max_size = 8, static _M_max_align = 4, _M_functor = {_M_unused = {_M_object = 0x200006f0, _M_const_object = 0x200006f0,
//      _M_function_pointer = 0x200006f0, _M_member_pointer = &virtual table offset 536872688, this adjustment -538444115}, _M_pod_data = "\360\006\000 [\375Ͽ"},
//      _M_manager = 0x80021b1 <std::_Function_base::_Base_manager<std::_Bind<void (SPI1Dirver::*(SPI1Dirver, std::_Placeholder<1>, std::_Placeholder<2>, std::_Placeholder<3>))(unsigned char const*, unsigned short, std::function<void (unsigned short)>)> >::_M_manager(std::_Any_data&, std::_Any_data const&, std::_Manager_operation)>},
//_M_invoker = 0x800216f <std::_Function_handler<void (unsigned char const*, unsigned short, std::function<void (unsigned short)>), std::_Bind<void (SPI1Dirver::*(SPI1Dirver, std::_Placeholder<1>, std::_Placeholder<2>, std::_Placeholder<3>))(unsigned char const*, unsigned short, std::function<void (unsigned short)>)> >::_M_invoke(std::_Any_data const&, unsigned char const*&&, unsigned short&&, std::function<void (unsigned short)>&&)>},
//
//
//
//
//    spi_async_write = {<std::_Maybe_unary_or_binary_function<void, unsigned char const*, short unsigned int, std::function<void(short unsigned int)> >> = {<No data fields>}, <std::_Function_base> = {static _M_max_size = 8, static _M_max_align = 4, _M_functor = {_M_unused = {_M_object = 0x20000748, _M_const_object = 0x20000748,
//      _M_function_pointer = 0x20000748, _M_member_pointer = (void (std::_Undefined_class::*)(std::_Undefined_class * const)) 0x20000748, this adjustment 268500964},
//      _M_pod_data = "H\a\000 \310\377\001 "},
//      _M_manager = 0x80021b1 <std::_Function_base::_Base_manager<std::_Bind<void (SPI1Dirver::*(SPI1Dirver, std::_Placeholder<1>, std::_Placeholder<2>, std::_Placeholder<3>))(unsigned char const*, unsigned short, std::function<void (unsigned short)>)> >::_M_manager(std::_Any_data&, std::_Any_data const&, std::_Manager_operation)>},
//_M_invoker = 0x800216f <std::_Function_handler<void (unsigned char const*, unsigned short, std::function<void (unsigned short)>), std::_Bind<void (SPI1Dirver::*(SPI1Dirver, std::_Placeholder<1>, std::_Placeholder<2>, std::_Placeholder<3>))(unsigned char const*, unsigned short, std::function<void (unsigned short)>)> >::_M_invoke(std::_Any_data const&, unsigned char const*&&, unsigned short&&, std::function<void (unsigned short)>&&)>},
//
//
//    delay = {<std::_Maybe_unary_or_binary_function<void, unsigned short>> = {<std::unary_function<unsigned short, void>> = {<No data fields>}, <No data fields>}, <std::_Function_base> = {static _M_max_size = 8, static _M_max_align = 4, _M_functor = {_M_unused = {_M_object = 0x8001629 <DelayMs>, _M_const_object = 0x8001629 <DelayMs>,
//      _M_function_pointer = 0x8001629 <DelayMs>, _M_member_pointer = &virtual table offset 134223401, this adjustment 67121307}, _M_pod_data = ")\026\000\b7a\000\b"},
//      _M_manager = 0x80022d5 <std::_Function_base::_Base_manager<void (*)(unsigned short)>::_M_manager(std::_Any_data&, std::_Any_data const&, std::_Manager_operation)>},
//_M_invoker = 0x80022a9 <std::_Function_handler<void (unsigned short), void (*)(unsigned short)>::_M_invoke(std::_Any_data const&, unsigned short&&)>},
//
//
//
//
//    spi_buffer = "\000\355w\377\b;R\v\360\001`\000\221\000\035", '\000' <repeats 17 times>}
//
//













































